#include "bsp.h"
#include "mrfi.h"
#include "bsp_leds.h"
#include "bsp_buttons.h"
#include "nwk_types.h"
#include "nwk_api.h"
#include "nwk_frame.h"
#include "nwk.h"
#include "vlo_rand.h"
#include "cod_errores.h"
#include "CalcPos.h"
#include "tabla.h"


void Init();
void TXString( char* cadena, int largo );
void systemStart();
void localizar(float distX,float distA, float distB);
void copiarArray(char* origen,char* destino, int largo );
void float2String(float numero,char*);

/** conexiones con cada nodo
*/
static          linkID_t sLinkIDX = 0;

/**
	Variables para guardar el ultimo valor del rssi
*/
static int rssi_intX;
static int rssi_intA;
static int rssi_intB;
static int rssi_intCA;
static int rssi_intCB;

static          char comandoSerie = 0;

static uint8_t sRxCallback(linkID_t port);


// main del nodo central
// 
int main( void )
{
  // Stop watchdog timer to prevent time out reset
  WDTCTL = WDTPW + WDTHOLD;
  
  Init();
  
  // inicializo el kit
  BSP_Init();
  
  
  TXString("Tracking v0.1\n",15);
  
  
  // levanto la radio -----------------------------------------------
  addr_t lAddr;
  
  // direccion del nodo central!!!!
  lAddr.addr[0]=0xAF; // nodo central
  lAddr.addr[1]=0;
  lAddr.addr[2]=0;
  lAddr.addr[3]=10;
  // ------------------------------
  // seteo la direccion del modulo
  // OJO que hay que reprogramar!!!!
  SMPL_Ioctl(IOCTL_OBJ_ADDR, IOCTL_ACT_SET, &lAddr);
  
  // esta llamada devuelve un fail, porque no es la idea que levante nada
  // asi que ni la chequeo...
  SMPL_Init(sRxCallback);
    
  // termine de levantar la radio
  TXString("Inicializacion finalizada\r\n",27);
  

  // espero conectarme con el nodoX
  while(SMPL_SUCCESS != SMPL_Link(&sLinkIDX));
    
  while (1)
  {  
    signalError(0);
    localizar(RSSIAMETRO(255-rssi_intX),RSSIAMETRO(255-rssi_intA),
              RSSIAMETRO(255-rssi_intB));    
  }
 
  
  
  return 0;
}

//-------------------------------------------------------------------
//
void localizar(float distX,float distA, float distB){

  char result1[4];// = "xx.x";
  char result2[4];// = "xx.x";
  
float dca = 2/RSSIAMETRO(rssi_intCA);
float dcb = 2/RSSIAMETRO(rssi_intCB);


  
  
  float2String(distX*(dca+dcb)/2,result1);
  TXString("DistX:",5);
  TXString(result1,4);
  TXString("\n\r",2);
  
  float2String(distA*dca,result1);
  TXString("DistA:",5);
  TXString(result1,4);
  TXString("\n\r",2);
  
  float2String(distB*dcb,result1);
  TXString("DistB:",5);
  TXString(result1,4);
  TXString("\n\r",2);
  
  
  
  coordenadas c = calcPos(2.0f,2.0f,distA,distB,distX);
  
  TXString("\n\rX:",4);  
  float2String(c.x,result1);
  TXString(result1,4);
  
  TXString("Y:",2);  
  float2String(c.y,result2);
  TXString(result2,4);
  
  TXString("\n\r",2); 
}

/** Convierte un float a un string para enviar por puerto serie
*/
void float2String(float numero, char* result){
  
    int nro = 10*numero;

    result[3] = '0'+nro%10;
    result[2] = '.';
    result[1] = '0'+(nro/10)%10;
    result[0] = '0'+(nro/100)%10;
  
  //return result;
}

// ------------------------------------------------------------------
// transmito por el puerto serie
//
/**
*	Transmite por el puerto serie
*	se pasa el string como array de char, y el largo
*	de caracteres validos en el array.
*	Si largo es mas grande que el del array, se arma
*/
void TXString( char* cadena, int largo )
{
  int puntero;
  for( puntero = 0; puntero < largo; puntero++){
    UCA0TXBUF = cadena[puntero];
    while (!(IFG2&UCA0TXIFG));              // espero a que termine de enviar
  }
}



// -------------------------------------------------
/**
*	Funcion de Callback que se llama en la ISR de la
*	radio. (Requisito de SimpliciTI)
*/
static uint8_t sRxCallback(linkID_t port){
  uint8_t msg[MAX_APP_PAYLOAD], len;
  len = 0;
    ioctlRadioSiginfo_t sigInfo;
    char rssiString[3];
    volatile signed int rssi_int;
    signed char rssi;
        char men[20];
  // manejo el mensaje dependiendo de quien lo envia
 

    // si recibo un paquete de largo mayor que 0
    if (SMPL_SUCCESS == SMPL_Receive(sLinkIDX, msg, &len) && len){
      sigInfo.lid = sLinkIDX;
      SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SIGINFO, (void *)&sigInfo);
      rssi = (signed char)sigInfo.sigInfo[0];
      rssi_intX = (signed int) rssi;
        rssi_intX += 128;
        //largoMsgX = len;
      rssi_intA = msg[2];
      rssi_intB = msg[3];
        
//      copiarArray(msg,msgX,len);  
      men[0] = '\r';
      men[1] = '\n';
      men[2] = 'X';
      men[5] = '0'+(rssi_intX%10);
      men[4] = '0'+((rssi_intX/10)%10);
      men[3] = '0'+((rssi_intX/100)%10);
      men[6] = '\n';
      men[7] = '\r';
      men[8] = 'A';
      men[11] = '0'+(rssi_intA%10);
      men[10] = '0'+((rssi_intA/10)%10);
      men[9] = '0'+((rssi_intA/100)%10);
      men[12] = '\n';
      men[13] = '\r';
      men[14] = 'B';
      men[17] = '0'+(rssi_intB%10);
      men[16] = '0'+((rssi_intB/10)%10);
      men[15] = '0'+((rssi_intB/100)%10);
      men[18] = '\n';
      men[19] = '\r';
            
      TXString(men,20);      
      
    }else if(port == SMPL_LINKID_USER_UUD){
    if ((SMPL_SUCCESS == SMPL_Receive(SMPL_LINKID_USER_UUD, msg, &len)) && len)
    {
      sigInfo.lid = SMPL_LINKID_USER_UUD;
      SMPL_Ioctl(IOCTL_OBJ_RADIO, IOCTL_ACT_RADIO_SIGINFO, (void *)&sigInfo);
      
      rssi = (signed char)sigInfo.sigInfo[0];
      
      if(msg[2] == 'A'){
        
        rssi_intCA = (signed int) rssi;
        rssi_intCA += 128;

        
      }else if(msg[2] == 'B'){
        
        rssi_intCB = (signed int) rssi;
        rssi_intCB += 128;

        
      }
    }
    }

  // libero el frame porque lo use aca...
  // guardo el ultimo mensaje, la unica opcion de que me envien
  // mas mensajes es que sean de WKUP y eso pasa solo en los nodos
  // moviles.
  // Como el codigo que ejecuta es corto lo dejo en el contexto
  // de la ISR
  return 1;
}

//---------------------------------------------------------------------------
/** INICIALIZACION DEL SISTEMA
*
*/
void Init()
{
  // SimpliciTI igual los cambia...
  P1DIR = 0xFB;
  P1OUT = 0x00;
  P2DIR = 0x27;
  P2OUT = 0x00;
  P3DIR = 0xC0;
  P3OUT = 0x00;
  P4DIR = 0xFF;
  P4OUT = 0x00;
  
  
  
  BCSCTL1 = CALBC1_8MHZ;                    // Set DCO
  DCOCTL = CALDCO_8MHZ;
  
  BCSCTL3 |= LFXT1S_2;                      // LFXT1 = VLO
  TACCTL0 = CCIE;                           // TACCR0 IE
  TACCR0 = 12000;                           // aprox un seg
  TACTL = TASSEL_1 + MC_1;                  // ACLK, upmode
  
  P3SEL |= 0x30;                            // P3.4,5 = USCI_A0 TXD/RXD
  UCA0CTL1 = UCSSEL_2;                      // SMCLK
  UCA0BR0 = 0x41;                           // 9600 a 8Mhz
  UCA0BR1 = 0x3;
  UCA0MCTL = UCBRS_2;                       
  UCA0CTL1 &= ~UCSWRST;                     // **Initialize USCI state achine**
  IE2 |= UCA0RXIE;                          // Enable USCI_A0 RX interrupt
  __enable_interrupt(); 
}

//---------------------------------------------------------------------
/** Timer A0 ISR
*/
#pragma vector=TIMERA0_VECTOR
__interrupt void Timer_A (void)
{
  // la idea era dormirse despues de x tiempo
}


///--------------------------------------------------------------------
/** ISR puerto serie
*/
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void)
{
  
  // me preparo para recibir comandos...
  
  char rx = UCA0RXBUF;
  if ( rx == 'b' || rx == 'B' )  // buscar nodo
  {
    comandoSerie = rx;
  }  
}

